import time
import math
import gc
from media.sensor import Sensor
from media.display import Display
from media.media import MediaManager
from ybUtils.YbUart import YbUart

# --- 初始化 ---
try:
    uart = YbUart(baudrate=115200)
    print("YbUart 初始化成功。")
except Exception as e:
    uart = None
    print(f"YbUart 初始化失败: {e}。将无法发送串口数据。")

def init_camera():
    """
    采用已验证的、能稳定运行的初始化流程
    """
    print("正在初始化摄像头 (采用已验证的纯净方案)...")
    sensor = Sensor()
    sensor.reset()
    sensor.set_framesize(width=640, height=480)
    sensor.set_pixformat(Sensor.RGB565)
    print("摄像头配置完成。")
    return sensor

def init_display(sensor_obj):
    """
    初始化显示设备
    """
    Display.init(Display.ST7701, to_ide=True)

def main():
    """
    运行二维码检测与数据发送的主函数 (优化数据格式版)
    """
    sensor = None
    # 1. 初始化
    try:
        sensor = init_camera()
        init_display(sensor)
        MediaManager.init()
        print("启动摄像头数据流...")
        sensor.run()
        print("数据流已启动。")
    except Exception as e:
        print(f"初始化或启动失败: {e}.")
        return

    if not uart:
        print("串口未能初始化，程序无法继续。")
        return

    # 2. 定义常量和帧计数器
    IMAGE_WIDTH = 640
    IMAGE_HEIGHT = 480
    IMAGE_CENTER_X = IMAGE_WIDTH / 2.0
    IMAGE_CENTER_Y = IMAGE_HEIGHT / 2.0

    clock = time.clock()
    FRAMES_PER_SEND = 5 # 每 5 帧发送一次数据
    frame_counter = 0

    print("\n--- 开始二维码检测 ---")
    print("警告：当前固件不支持图像增强，请务必通过'手动调焦'和'加强光照'来提高识别率！")

    try:
        while True:
            clock.tick()
            frame_counter += 1

            img = sensor.snapshot()
            img.to_grayscale()
            qr_codes = img.find_qrcodes()

            if qr_codes:
                qr = qr_codes[0]
                qr_id = qr.payload()
                rect = qr.rect()

                img.draw_rectangle(rect, thickness=2, color=(200, 0, 0))
                img.draw_string_advanced(0, 0, 30, qr_id, color=(255, 255, 255))

                # 使用取模运算判断是否到达发送时机
                if frame_counter % FRAMES_PER_SEND == 0:
                    # --- 核心修改：计算误差值 ---
                    x, y, w, h = rect
                    corners = qr.corners()

                    # 1. 计算二维码中心点
                    qr_center_x = x + w/2
                    qr_center_y = y + h/2

                    # 2. 计算相对于图像中心的偏移误差
                    x_offset = qr_center_x - IMAGE_CENTER_X
                    y_offset = qr_center_y - IMAGE_CENTER_Y

                    # 3. 计算角度偏移误差
                    angle_offset = 0
                    if len(corners) >= 2:
                        # 注意：为了让“顺时针偏转为正”，这里需要对y取反
                        dx = corners[1][0] - corners[0][0]
                        dy = corners[1][1] - corners[0][1]
                        angle_offset = math.degrees(math.atan2(-dy, dx))

                    # 4. 构建并发送新的“误差”数据包
                    # 格式: $ERR,ID,X_Offset,Y_Offset,Angle_Offset\n
                    data_str = f"$ERR,{qr_id},{x_offset:.1f},{y_offset:.1f},{angle_offset:.1f}\n"

                    print(f"FPS: {clock.fps():.1f} | 帧计数到达({frame_counter})，发送误差数据: {data_str.strip()}")
                    uart.uart.write(data_str.encode('utf-8'))
                else:
                    print(f"FPS: {clock.fps():.1f} | 检测到二维码，等待下一发送帧... (当前帧: {frame_counter})")

            else:
                print(f"FPS: {clock.fps():.1f} | 未检测到二维码...")

            Display.show_image(img)

    except KeyboardInterrupt:
        print("程序被用户终止。")
    except Exception as e:
        print(f"主循环中发生错误: {e}")
    finally:
        # 清理资源
        Display.deinit()
        MediaManager.deinit()
        gc.collect()
        print("资源已清理。")

if __name__ == "__main__":
    main()
